#include <QtGui>

#include "rawdatawidget.h"
#include "coordinate3D.h"
#include "vector3D.h"
#include "robot.h"
#include "core.h"

#include <QDebug>

rawdataWidget::rawdataWidget(Robot *robot, Core *core)
{
	robotPointer = robot;
	corePointer = core;

	roundPrecision = 10000;
	
	tableWidget = new QTableWidget(7, 13, this);

	tableWidget->setHorizontalHeaderLabels(QStringList()<<"Joint Number"<<"Joint Type"<<"Actuation Level"<<"X Position"<<"Y Position"<<
	"Z Position"<<"X Orientation"<<"Y Orientation"<<"Z Orientation"<<"DH Trans K"<<"DH Rotation K"<<"DH Trans I"<<"DH Rotation I");
	tableWidget->setVerticalHeaderLabels(QStringList()<<"0"<<"1"<<"2"<<"3"<<"4"<<"5"<<"6");
	tableWidget->resize(800, 250);
	
	
	checkBoxActuation = new QCheckBox(QString("Show actuation level"));
	checkBoxPosition = new QCheckBox(QString("Show Joint center position"));
	checkBoxOrientation = new QCheckBox(QString("Show Joint K Axis orientation"));
	checkBoxDHParams = new QCheckBox(QString("Show Joint DH parameters"));	
	
	optionsLayout = new QGridLayout;
	optionsLayout->addWidget(checkBoxActuation, 0, 0);
	optionsLayout->addWidget(checkBoxPosition, 1, 0);
	optionsLayout->addWidget(checkBoxOrientation, 0, 2);
	optionsLayout->addWidget(checkBoxDHParams, 1, 2);
	
	connect(checkBoxActuation, SIGNAL(stateChanged(int)), this, SLOT(showActuation(int)));
	connect(checkBoxPosition, SIGNAL(stateChanged(int)), this, SLOT(showPosition(int)));
	connect(checkBoxOrientation, SIGNAL(stateChanged(int)), this, SLOT(showOrientation(int)));
	connect(checkBoxDHParams, SIGNAL(stateChanged(int)), this, SLOT(showDHParams(int)));
	
	checkBoxActuation->setCheckState(Qt::Checked);
	checkBoxPosition->setCheckState(Qt::Checked);
	checkBoxOrientation->setCheckState(Qt::Checked);
	checkBoxDHParams->setCheckState(Qt::Checked);
	
	
	buttonsLayout = new QHBoxLayout;
	refreshButton = new QPushButton(QString("Refresh"));
	calculateButton = new QPushButton(QString("Calculate"));
	exportButton = new QPushButton(QString("Export"));
	
	connect(refreshButton, SIGNAL(clicked()), this, SLOT(resetTable()));
	connect(calculateButton, SIGNAL(clicked()), this, SLOT(calculate()));
	connect(exportButton, SIGNAL(clicked()), this, SLOT(exportData()));
	
	
	buttonsLayout->addWidget(refreshButton);
	buttonsLayout->addWidget(calculateButton);
	buttonsLayout->addWidget(exportButton);
	
	mainLayout = new QVBoxLayout(this);
	mainLayout->setSpacing(20);
	mainLayout->addWidget(tableWidget);
	mainLayout->addLayout(optionsLayout);
	mainLayout->addLayout(buttonsLayout);
	mainLayout->addStretch(0);
	
	this->setLayout(mainLayout);
	
	resetTable();
}

void rawdataWidget::setTabIndex(int index)	//SLOT
{
	tabIndex = index;
}

void rawdataWidget::robotLoaded()	//SLOT
{	
	resetTable();
}

void rawdataWidget::tabSelected(int index)
{
	if (index == tabIndex)
		resetTable();
}

void rawdataWidget::showActuation(int state)
{
	if (state == Qt::Checked)
		tableWidget->showColumn(2);
	else
		tableWidget->hideColumn(2);
}

void rawdataWidget::showPosition(int state)
{
	if (state == Qt::Checked)
	{	
		tableWidget->showColumn(3);
		tableWidget->showColumn(4);
		tableWidget->showColumn(5);
	}
	else
	{
		tableWidget->hideColumn(3);
		tableWidget->hideColumn(4);
		tableWidget->hideColumn(5);
	}
}

void rawdataWidget::showOrientation(int state)
{
	if (state == Qt::Checked)
	{	tableWidget->showColumn(6);
		tableWidget->showColumn(7);
		tableWidget->showColumn(8);
	}
	else
	{
		tableWidget->hideColumn(6);
		tableWidget->hideColumn(7);
		tableWidget->hideColumn(8);
	}
}

void rawdataWidget::showDHParams(int state)
{
	if (state == Qt::Checked)
	{	tableWidget->showColumn(9);
		tableWidget->showColumn(10);
		tableWidget->showColumn(11);
		tableWidget->showColumn(12);
	}
	else
	{
		tableWidget->hideColumn(9);
		tableWidget->hideColumn(10);
		tableWidget->hideColumn(11);
		tableWidget->hideColumn(12);
	}
}
	
void rawdataWidget:: resetTable()	
{	//refetch all values and hide selected columns
	tableWidget->clearContents();
	
	coord3D center;
	vector3D orientation;

	for (int i=0; i<robotPointer->jointList.size(); i++)
	{
		QTableWidgetItem *newItem = new QTableWidgetItem(tr("%1").arg(i));
		tableWidget->setItem(i, 0, newItem);
		
		switch (robotPointer->jointList[i].getType())
		{
			case 0:
			{	newItem = new QTableWidgetItem(tr("Joint"));
				tableWidget->setItem(i, 1, newItem);
				 break;
			}
			case 1:
			{	newItem = new QTableWidgetItem(tr("Revolute"));
				tableWidget->setItem(i, 1, newItem);
				 break;
			}
			case 2:
			{	newItem = new QTableWidgetItem(tr("Prismatic"));
				tableWidget->setItem(i, 1, newItem);
				 break;
			}
			case 3:
			{	newItem = new QTableWidgetItem(tr("Gripper"));
				tableWidget->setItem(i, 1, newItem);
				 break;
			}
			case 4:
			{	newItem = new QTableWidgetItem(tr("Welder"));
				tableWidget->setItem(i, 1, newItem);
				 break;
			}
			default:
			{	newItem = new QTableWidgetItem(tr("Unknown"));
				tableWidget->setItem(i, 1, newItem);
				 break;
			}
		}
			 
		newItem = new QTableWidgetItem(tr("%1").arg(round(robotPointer->jointList[i].getCurrentActuation())));
		tableWidget->setItem(i, 2, newItem);
		
		center = robotPointer->jointList[i].getCenter();
		orientation = robotPointer->jointList[i].orientationK();
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(center.x())));
		tableWidget->setItem(i, 3, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(center.y())));
		tableWidget->setItem(i, 4, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(center.z())));
		tableWidget->setItem(i, 5, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(orientation.x())));
		tableWidget->setItem(i, 6, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(orientation.y())));
		tableWidget->setItem(i, 7, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(orientation.z())));
		tableWidget->setItem(i, 8, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(robotPointer->jointList[i].dhTranslationK())));
		tableWidget->setItem(i, 9, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(robotPointer->jointList[i].dhRotationK())));
		tableWidget->setItem(i, 10, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(robotPointer->jointList[i].dhTranslationI())));
		tableWidget->setItem(i, 11, newItem);
		
		newItem = new QTableWidgetItem(tr("%1").arg(round(robotPointer->jointList[i].dhRotationI())));
		tableWidget->setItem(i, 12, newItem);
	}
	tableWidget->resizeColumnsToContents();
}

void rawdataWidget::calculate()
{
	bool ok;
	
	//go through the table, pick out modified actuation levels and use them to recalculate robot
	//Qvariants are nifty!
	for (int i=0; i< robotPointer->jointList.size(); i++)
	{
		corePointer->adjustJoint(i, float(tableWidget->item(i, 2)->data(Qt::DisplayRole).toDouble(&ok)));
	}
	
	resetTable();
}

void rawdataWidget::exportData()
{
	
	int exportFlags = 0;
	
	if (checkBoxActuation->checkState() == Qt::Checked)
		exportFlags |= 0x1;
	if (checkBoxPosition->checkState() == Qt::Checked)
		exportFlags |= 0x2;
	if (checkBoxOrientation->checkState() == Qt::Checked)
	 	exportFlags |= 0x4;
	if (checkBoxDHParams->checkState() == Qt::Checked)
	 	exportFlags |= 0x8;
	
	corePointer->exportData(exportFlags);
}

float rawdataWidget::round(float number)
{
	float returnValue;
	
	returnValue = float(floor(number * roundPrecision))/float(roundPrecision);
	
	return returnValue;
}

